//
// Created by clarence on 2021/5/13.
//

#include <ros/ros.h>

#include <tf/transform_datatypes.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/time_synchronizer.h>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Point32.h>
#include <std_msgs/Float64MultiArray.h>
#include <sensor_msgs/PointCloud2.h>
#include <visualization_msgs/Marker.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <iostream>
#include <string>
#include <cmath>
#include <algorithm>
#include <tf/transform_broadcaster.h>
#include <time.h>
#include <stdlib.h>
#include <mavros_msgs/State.h>
#include <random>
#include <nav_msgs/Odometry.h>
#include <control_msgs/JointControllerState.h>
#include <std_msgs/Float64.h>
#include <queue>
#include <vector>
#include <fstream>
#include <cstdint>
#include <trajectory_msgs/JointTrajectoryPoint.h>
#include "vied_map_array_improve_5_cluster_velocity_zyx.h"
#include "gazebo_msgs/ModelStates.h"
#include "gazebo_msgs/ModelState.h"

using namespace message_filters;
using namespace std;

#define PIx2 6.28318
#define PI 3.14159
#define PI_2 1.5708


const float CAL_DURATION = 0.05f; // 20Hz

const int ANGLE_H_NUM = 17;
const int ANGLE_V_NUM = 7;

float MAX_V_XY = 1.f;
float MAX_V_Z_UP = 1.f;
float MAX_V_Z_DOWN = 0.5f;
float MAX_A = 1.5f;
float HEIGHT_LIMIT = 2.f;
float XY_LIMIT = 2.5f;

float MAX_RISK_ONE_PRIMITIVE = 20.0;

float DIRECTION_CHANGE_LEFT_SIZE = 1.5;

float PLAN_INIT_STATE_CHANGE_THRESHOLD = 0.1;

bool DIRECTION_AUTO_CHANGE = false;

struct  Path_Planning_Parameters
{
    float d_ref = 1.6;  //NOTE: NEED TO CHANGE CHG
    float k1_xy = 5; //% Goal directed coefficient
    float k1_z = 3; //% Goal directed coefficient
    float k2_xy = 1; //% Rotation coefficient 3
    float k2_z = 1.5; //% Rotation coefficient 3.5
    float v_max_at_goal_ori = 0.3; //% m/s, just reference  5.0 originally
    float v_scale_min = 0.1;
    float collision_threshold_static = 0.6;
    float collision_threshold_dynamic = 1.0;
    int max_plan_num = ANGLE_H_NUM * ANGLE_V_NUM;  // Previously it was 100, as 18*7 = 126 > 100
}pp;


float MAP_DELAY_SECONDS = 0.05;
const int point_num_pub = 5; // For the visualization of current planned trajectory

bool offboard_ready = false;
bool state_locked = false;
bool state_updating = false;
bool safe_trajectory_avaliable = true;
bool use_position_global_time = false;
bool if_plan_vertical_path = true;
bool if_flyback_enable = true;
bool if_in_simulation = true;
bool trajectory_initialized = false;


std::queue<double> pose_timestamp_queue;
std::queue<Eigen::Quaternionf> att_queue;
std::queue<Eigen::Vector3d> pos_queue;

/****** Global variables for path planning ******/
Eigen::VectorXf Fov_half(2); //Fov parameters
Eigen::VectorXf Angle_h(ANGLE_H_NUM);  // initiate later in the main function, rad
Eigen::VectorXf Angle_v(ANGLE_V_NUM); // initiate later in the main function, rad

Eigen::Vector3d p_goal;
Eigen::Vector3d p_goal_raw;
Eigen::Vector3d p0;
Eigen::Vector3d v0;
Eigen::Vector3d a0;
Eigen::Quaternionf quad(1.0, 0.0, 0.0, 0.0);
Eigen::Vector3d init_p;
float yaw0;
float yaw0_rate;

float theta_h_last = 0.0;
float theta_v_last = 0.0;

vector<Eigen::Vector3d> traj_p_buffer_1, traj_p_buffer_2;
vector<Eigen::Vector3d> traj_v_buffer_1, traj_v_buffer_2;
vector<Eigen::Vector3d> traj_a_buffer_1, traj_a_buffer_2;

Eigen::MatrixXd traj_p_buffer_2_matrix;

bool trajectory_buffer_1_requires_update = true;
bool trajectory_buffer_2_requires_update = true;
int trajectory_1_send_counter = 0;

Eigen::Vector3d last_send_p, last_send_v, last_send_a;


struct ActorState{
    string name;
    double record_time;
    double px = 0.f;
    double py = 0.f;
    double pz = 0.f;
    double yaw = 0.f;
    double vx = 0.f;
    double vy = 0.f;
    double vz = 0.f;
};

vector<ActorState> actor_states;
vector<ActorState> actor_avg_states;

ros::Publisher pva_pub, current_marker_pub;

void setParameters();

void marker_publish(const Eigen::MatrixXd &Points, int id, float r, float g, float b, float width, int publish_num);

void marker_publish(const vector<Eigen::Vector3d> &Points, int id, float r, float g, float b, float width, int publish_num);

/**********************FOR HSH TABLE***************************/
// PVA table for feasible total planning time T
typedef struct pva_table pva_table;
struct pva_table {
    int dim_num;
    int *dim_size;
    uint64_t *dim_interval;
    uint64_t table_size;
    float *table;
    float rez;
    float *pva_limit;

    void construct_pva_table(int dim1_size, int dim2_size, int dim3_size, int dim4_size, float resolution) {
        this->dim_num = 4;
        this->dim_size = (int*)malloc(sizeof(int)*this->dim_num);
        this->dim_interval = (uint64_t*)malloc(sizeof(uint64_t)*this->dim_num);

        this->dim_size[0] = dim1_size;
        this->dim_size[1] = dim2_size;
        this->dim_size[2] = dim3_size;
        this->dim_size[3] = dim4_size;

        this->dim_interval[3] = 1;
        this->dim_interval[2] = dim_interval[3] * dim4_size;
        this->dim_interval[1] = dim_interval[2] * dim3_size;
        this->dim_interval[0] = dim_interval[1] * dim2_size;

        this->table_size = this->dim_interval[0] * dim1_size;
        this->table = (float*)malloc(sizeof(float)*this->table_size);

        this->rez = resolution;

        this->pva_limit = (float*)malloc(sizeof(float)*3);
        this->pva_limit[0] = this->rez*float(dim1_size/2);
        this->pva_limit[1] = this->rez*float(dim2_size/2);
        this->pva_limit[2] = this->rez*float(dim4_size/2);
    }

    void compute_idx_from_pva(float dlt_p, float v0, float vf, float a0,
                              int &idx1, int &idx2, int &idx3, int &idx4) {
        idx1 = round(dlt_p/this->rez) + this->dim_size[0]/2;
        idx2 = round(v0/this->rez) + this->dim_size[1]/2;
        idx3 = round(vf/this->rez) + this->dim_size[2]/2;
        idx4 = round(a0/this->rez) + this->dim_size[3]/2;
    }

    float query_pva_table(float dlt_p, float v0, float vf, float a0) {
        if (fabs(dlt_p) > this->pva_limit[0]) static_assert("Invalid input!", "");
        if (fabs(v0) > this->pva_limit[1]) static_assert("Invalid input!", "");
        if (fabs(vf) > this->pva_limit[1]) static_assert("Invalid input!", "");
        if (fabs(a0) > this->pva_limit[2]) static_assert("Invalid input!", "");

        int idx1, idx2, idx3, idx4;
        this->compute_idx_from_pva(dlt_p, v0, vf, a0, idx1, idx2, idx3, idx4);

        uint64_t idx = idx1*this->dim_interval[0] + idx2*this->dim_interval[1] +
                       idx3*this->dim_interval[2] + idx4*this->dim_interval[3];

        // std::cout << "idx: " << idx << std::endl;

        return this->table[idx];
    }

    void pva_table2csv(const std::string &str) {
        std::ofstream outfile;
        outfile.open(str, std::ios::out);

        for (int i = 0; i < 4; ++i) outfile << std::to_string(this->dim_size[i]) << ',';
        outfile << std::to_string(this->rez) << std::endl;

        for (uint64_t i = 0; i < this->table_size-1; ++i) outfile << std::to_string(this->table[i]) << ',';
        outfile << std::to_string(this->table[table_size-1]);

        outfile.close();
    }

    void csv2pva_table(const std::string &str) {
        int tmp_dim_size[4];
        float tmp_rez;

        std::ifstream infile(str, std::ios::in);
        std::string tmp_str;

        for (int i = 0; i < 4; ++i) {
            getline(infile, tmp_str, ',');
            tmp_dim_size[i] = std::stoi(tmp_str);
        }

        getline(infile, tmp_str);
        tmp_rez = std::stod(tmp_str);

        this->construct_pva_table(tmp_dim_size[0], tmp_dim_size[1],
                                  tmp_dim_size[2], tmp_dim_size[3], tmp_rez);

        for (uint64_t i = 0; i < this->table_size; ++i) {
            getline(infile, tmp_str, ',');
            this->table[i] = std::stod(tmp_str);
        }
    }

    void free_pva_table() {
        free(this->pva_limit);
        free(this->table);
        free(this->dim_interval);
        free(this->dim_size);
    }
};

pva_table *trajectory_time_table = (pva_table*)malloc(sizeof(pva_table));
/***************************************************/

/** This function is to generate state to state trajectory **/
bool motion_primitives_with_table(Eigen::Vector3d p0, Eigen::Vector3d v0, Eigen::Vector3d a0, float yaw0, float theta_h,
                                  float theta_v, Eigen::Vector3d goal, float d, float v_final, float delt_t,
                                  Eigen::MatrixXd &p, Eigen::MatrixXd &v, Eigen::MatrixXd &a, Eigen::VectorXd &t)
{
    float delt_x = d*cos(theta_v)*cos(theta_h+yaw0);
    float delt_y = d*cos(theta_v)*sin(theta_h+yaw0);
    float delt_z = d*sin(theta_v);

    Eigen::Vector3d pf;
    pf << p0(0)+delt_x, p0(1)+delt_y, p0(2)+delt_z;

    Eigen::Vector3d l = goal - pf;
    Eigen::Vector3d vf = (v_final / l.norm()) * l;
    vf(2) = 0; // % Note: 0 maybe better, for the p curve wont go down to meet the vf

    Eigen::Vector3d af = Eigen::Vector3d::Zero();

    float j_limit = 4;
    float a_limit = MAX_A;
    float v_limit = MAX_V_XY;

    for(int i=0; i<3; i++){
        if(fabs(v0(i)) > MAX_V_XY) {
            v0(i) = MAX_V_XY * v0(i) / fabs(v0(i));
            ROS_WARN_THROTTLE(1, "v_limit in local planning is large than the values in the loaded table!  v0 max=%f", v0.cwiseAbs().maxCoeff());
        }
        if(fabs(a0(i)) > MAX_A) {
            a0(i) = MAX_A * a0(i) / fabs(a0(i));
            ROS_WARN_THROTTLE(1, "a_limit in local planning is large than the values in the loaded table!  a0 max=%f", a0.cwiseAbs().maxCoeff());
        }
    }

    float T1, T2, T3, T;
    T1 = trajectory_time_table->query_pva_table(delt_x, v0(0), vf(0), a0(0));
    T2 = trajectory_time_table->query_pva_table(delt_y, v0(0), vf(0), a0(0));
    T3 = trajectory_time_table->query_pva_table(delt_z, v0(0), vf(0), a0(0));

    if (T1 == -1 || T2 == -1 || T3 == -1) {
        return false;
    }

    T = T1 > T2 ? T1 : T2;
    T = T > T3 ? T : T3;
//    T *= 1.1;
    T = T < 0.5 ? 0.5 : T;

    int times = T / delt_t;

    p = Eigen::MatrixXd::Zero(times, 3);
    v = Eigen::MatrixXd::Zero(times, 3);
    a = Eigen::MatrixXd::Zero(times, 3);
    t = Eigen::VectorXd::Zero(times);

    // % calculate optimal jerk controls by Mark W. Miller

    for(int ii=0; ii<3; ii++)
    {
        float delt_a = af(ii) - a0(ii);
        float delt_v = vf(ii) - v0(ii) - a0(ii)*T;
        float delt_p = pf(ii) - p0(ii) - v0(ii)*T - 0.5*a0(ii)*T*T;

        // % if vf is not free
        float alpha = delt_a*60/pow(T,3) - delt_v*360/pow(T,4) + delt_p*720/pow(T,5);
        float beta = -delt_a*24/pow(T,2) + delt_v*168/pow(T,3) - delt_p*360/pow(T,4);
        float gamma = delt_a*3/T - delt_v*24/pow(T,2) + delt_p*60/pow(T,3);

        for(int jj=0; jj<times; jj++)
        {
            float tt = (jj+1) * delt_t;
            t(jj) = tt;
            p(jj,ii) = alpha/120*pow(tt,5) + beta/24*pow(tt,4) + gamma/6*pow(tt,3) + a0(ii)/2*pow(tt,2) + v0(ii)*tt + p0(ii);
            v(jj,ii) = alpha/24*pow(tt,4) + beta/6*pow(tt,3) + gamma/2*pow(tt,2) + a0(ii)*tt + v0(ii);
            a(jj,ii) = alpha/6*pow(tt,3) + beta/2*pow(tt,2) + gamma*tt + a0(ii);

            // // Only for checking acceleration
            // if (fabs(a(jj, ii)) > planning_max_accel(ii)) planning_max_accel(ii) = fabs(a(jj, ii));
            // if (fabs(v(jj, ii)) > planning_max_vel(ii)) planning_max_vel(ii) = fabs(v(jj, ii));
        }
    }

    return true;
}


bool collisionCheckingDynamic(const vector<Eigen::Vector3d> &p_left, const Eigen::Vector3d &start_p, float dynamic_obstacle_range_limit = 5.f){
    // Filter out close obstacles
    vector<ActorState> close_actors;
    for(auto &actor : actor_avg_states){
        /// TODO: USE 3D distance
        double distance_2d = sqrt((start_p(0) - actor.px)*(start_p(0) - actor.px) +
                                  (start_p(1) - actor.py)*(start_p(1) - actor.py));
        if(distance_2d <= dynamic_obstacle_range_limit){
            close_actors.push_back(actor);
        }
    }

    // Check collision
    for(int i=0; i < p_left.size(); ++i){
        double check_t = i * CAL_DURATION;
        /// TODO: USE 3D distance
        double check_px = p_left[i](0);
        double check_py = p_left[i](1);
        double check_pz = p_left[i](2);

        if(check_pz > HEIGHT_LIMIT){
            return false;
        }

        for(auto const &actor : close_actors){
            double actor_px_t = actor.px + actor.vx * check_t;
            double actor_py_t = actor.py + actor.vy * check_t;

            double check_distance_2d = sqrt((check_px-actor_px_t)*(check_px-actor_px_t) + (check_py-actor_py_t)*(check_py-actor_py_t));
            if(check_distance_2d < pp.collision_threshold_dynamic){
                return false;
            }
        }

    }

    return true;
}


bool collisionCheckingAndRiskPredictionDynamic(const Eigen::MatrixXd &p, const Eigen::Vector3d &current_p, float &risk, double absolute_safe_time = 1.f, float dynamic_obstacle_range_limit = 5.f){
    // Filter out close obstacles
    vector<ActorState> close_actors;
    for(auto &actor : actor_avg_states){
        /// TODO: USE 3D distance
        double distance_2d = sqrt((current_p(0) - actor.px)*(current_p(0) - actor.px) +
                               (current_p(1) - actor.py)*(current_p(1) - actor.py));
        if(distance_2d <= dynamic_obstacle_range_limit){
            close_actors.push_back(actor);
        }
    }

    // Calculate checkin range
    int collision_checking_times = absolute_safe_time / CAL_DURATION;
    if(collision_checking_times > p.rows()){
        collision_checking_times = p.rows();
    }
    // Check collision
    for(int i=0; i < collision_checking_times; ++i){
        double check_t = i * CAL_DURATION;
        double check_px = p(i, 0);
        double check_py = p(i, 1);
        double check_pz = p(i, 2);
        /// TODO: USE 3D distance

        if(check_pz > HEIGHT_LIMIT){
            return false;
        }

        for(auto const &actor : close_actors){
            double actor_px_t = actor.px + actor.vx * check_t;
            double actor_py_t = actor.py + actor.vy * check_t;

            double check_distance_2d = sqrt((check_px-actor_px_t)*(check_px-actor_px_t) + (check_py-actor_py_t)*(check_py-actor_py_t));
            if(check_distance_2d < pp.collision_threshold_dynamic){
                return false;
            }
        }

    }

    // Calculate future risk
    risk = 0.f;
    static const float risk_distance_threshold = 2 * pp.collision_threshold_dynamic;
    static const float risk_quad_function_coefficient = -1.f / (risk_distance_threshold * risk_distance_threshold);

    for(int i=collision_checking_times; i < p.rows(); ++i){
        float check_t = i * CAL_DURATION;;
        float check_px = p(i, 0);
        float check_py = p(i, 1);
//        double check_pz = p(i, 2);

        float check_point_risk = 0.0;

        for(auto const &actor : close_actors){
            float actor_px_t = actor.px + actor.vx * check_t;
            float actor_py_t = actor.py + actor.vy * check_t;

            float check_distance_2d_square = (check_px-actor_px_t)*(check_px-actor_px_t) + (check_py-actor_py_t)*(check_py-actor_py_t);
            if(check_distance_2d_square < risk_distance_threshold * risk_distance_threshold){
                check_point_risk += risk_quad_function_coefficient * check_distance_2d_square + 1.f;

                if(check_point_risk > 1.0){
                    check_point_risk = 1.0;
                    break;
                }
            }
        }

        risk += check_point_risk;
    }

    // Set a risk limitation
    if(risk > MAX_RISK_ONE_PRIMITIVE){
        risk = MAX_RISK_ONE_PRIMITIVE;
    }

    return true;
}


bool trajectoryPlanOnePiece(Eigen::Vector3d p_start, Eigen::Vector3d v_start, Eigen::Vector3d a_start, float yaw_start,
                            Eigen::MatrixXd &traj_p_buffer_chosen, Eigen::MatrixXd &traj_v_buffer_chosen,
                            Eigen::MatrixXd &traj_a_buffer_chosen, int piece_seq = 1, float absolute_safe_time = 1.f){

    static bool first_plan = true;
    static float last_angle_h = 0.f;
    static float last_angle_v = 0.f;
    float angle_h_chosen = 0.f;
    float angle_v_chosen = 0.f;

    vector<Eigen::MatrixXd> primitive_candidate_p;
    vector<Eigen::MatrixXd> primitive_candidate_v;
    vector<Eigen::MatrixXd> primitive_candidate_a;
    vector<float> primitive_candidate_risk;
    vector<float> primitive_candidate_angle_h;
    vector<float> primitive_candidate_angle_v;

    Eigen::Vector3d delt_p = p_goal - p_start;
    float phi_h = atan2(delt_p(1), delt_p(0)); //% horizental offset angle
    float phi_v = atan2(delt_p(2), sqrt(delt_p(0) * delt_p(0) + delt_p(1) * delt_p(1))); //% vertical offset angle


    for(int i=0; i<ANGLE_H_NUM; i++)
    {
        for(int j=0; j<ANGLE_V_NUM; j++)
        {
            float angle_h_this = Angle_h(i) + phi_h;
            float angle_v_this = Angle_v(j) + phi_v;

            Eigen::MatrixXd p;
            Eigen::MatrixXd v;
            Eigen::MatrixXd a;
            Eigen::VectorXd t;

            if(motion_primitives_with_table(p_start, v_start, a_start, yaw_start, angle_h_this, angle_v_this, p_goal,
                                            pp.d_ref, pp.v_max_at_goal_ori, CAL_DURATION, p, v, a, t)){

                // Collision checking and risk prediction
                float risk = 0.0;
                if(collisionCheckingAndRiskPredictionDynamic(p, p_start, risk, absolute_safe_time)){
                    primitive_candidate_p.push_back(p);
                    primitive_candidate_v.push_back(v);
                    primitive_candidate_a.push_back(a);
                    primitive_candidate_risk.push_back(risk);
                    primitive_candidate_angle_h.push_back(angle_h_this);
                    primitive_candidate_angle_v.push_back(angle_v_this);
                }
            }

        }
    }

    if(primitive_candidate_p.empty()){
        ROS_INFO("No safe path");
        return false;
    }

    // Find the best trajectory
    float cost_min = 100000.f;

    for(int i=0; i<primitive_candidate_p.size(); ++i){
        float cost_goal = pp.k1_xy * (primitive_candidate_angle_h[i] - phi_h) * (primitive_candidate_angle_h[i] - phi_h) +
                          pp.k1_z * (primitive_candidate_angle_v[i] - phi_v) * (primitive_candidate_angle_v[i] - phi_v);

        float cost_direction_change = 0.f;
        if(!first_plan && piece_seq == 1){  // only for the first piece
            float delt_angle_h = primitive_candidate_angle_h[i]  - last_angle_h;
            float delt_angle_v = primitive_candidate_angle_v[i]  - last_angle_v;
            cost_direction_change = pp.k2_xy * delt_angle_h*delt_angle_h + pp.k2_z * delt_angle_v*delt_angle_v;
        }

        float total_cost = cost_goal + cost_direction_change + primitive_candidate_risk[i];
        if(total_cost < cost_min){
            cost_min = total_cost;
            traj_p_buffer_chosen = primitive_candidate_p[i];
            traj_v_buffer_chosen = primitive_candidate_v[i];
            traj_a_buffer_chosen = primitive_candidate_a[i];
        }
    }

    if(piece_seq == 1){  // only for the first piece
        first_plan = false;
        last_angle_h = angle_h_chosen;
        last_angle_v = angle_v_chosen;
    }

    return true;
}

void trajectoryCallback(const ros::TimerEvent& e) {
//    static Eigen::MatrixXd whole_traj1_to_visualize, whole_traj2_to_visualize;

    double planning_start_time = ros::Time::now().toSec();

    static const float absolute_safe_time = 1.0f;
    static const int points_num_one_piece = (int)(absolute_safe_time / CAL_DURATION);

    Eigen::Vector3d current_p = p0;
    Eigen::Vector3d current_v = v0;
    Eigen::Vector3d current_a = a0;
    float current_yaw = yaw0;

    if(!trajectory_initialized){
        last_send_p = current_p;
        last_send_v << 0.0, 0.0, 0.0;
        last_send_a << 0.0, 0.0, 0.0;
    }


    /// Left points piece one
    vector<Eigen::Vector3d> left_points_piece_one;
    for(int i=trajectory_1_send_counter; i<traj_p_buffer_1.size(); ++i){
        left_points_piece_one.emplace_back(traj_p_buffer_1[i]);
    }
    // Visualization
    marker_publish(left_points_piece_one, 3, 0.f, 1.f, 0.f, 0.1, 8);


    /// Check if need to replace trajectory 1 with 2.
    static const int transfer_buffer_size = 5;
    if(trajectory_initialized && trajectory_1_send_counter >= traj_p_buffer_1.size() - transfer_buffer_size){ /// frequency and calculation frequency usually don't match. Use buffer with transfer_buffer_size points.

        if(!traj_p_buffer_2.empty()){
            vector<Eigen::Vector3d> transfer_buffer_p_temp;
            vector<Eigen::Vector3d> transfer_buffer_v_temp;
            vector<Eigen::Vector3d> transfer_buffer_a_temp;
            for(int j= traj_p_buffer_1.size() - trajectory_1_send_counter; j>0; --j){
                transfer_buffer_p_temp.push_back(traj_p_buffer_1[traj_p_buffer_1.size() - j]);
                transfer_buffer_v_temp.push_back(traj_v_buffer_1[traj_v_buffer_1.size() - j]);
                transfer_buffer_a_temp.push_back(traj_a_buffer_1[traj_a_buffer_1.size() - j]);
            }

            traj_p_buffer_1 = transfer_buffer_p_temp;
            traj_v_buffer_1 = transfer_buffer_v_temp;
            traj_a_buffer_1 = transfer_buffer_a_temp;
            trajectory_1_send_counter = 0;

            int new_start_seq = traj_p_buffer_1.size();

            for(int i=0; i<traj_p_buffer_2.size(); ++i){  // 0 is ignore because it is the same point as the last point in original traj1
                traj_p_buffer_1.push_back(traj_p_buffer_2[i]);
                traj_v_buffer_1.push_back(traj_v_buffer_2[i]);
                traj_a_buffer_1.push_back(traj_a_buffer_2[i]);
            }

            trajectory_buffer_2_requires_update = true;

            ROS_INFO("Replaced Traj1 with Traj2, tracked_point=(%f, %f, %f), new_point=(%f, %f, %f), tracked_vel=(%f, %f, %f), new_vel=(%f, %f, %f)",
                     last_send_p(0),last_send_p(1),last_send_p(2), traj_p_buffer_1[0](0), traj_p_buffer_1[0](1), traj_p_buffer_1[0](2),
                     last_send_v(0), last_send_v(1), last_send_v(2), traj_v_buffer_1[0](0), traj_v_buffer_1[0](1), traj_v_buffer_1[0](2));
        }else{
            ROS_INFO("Replaced Traj1 with Traj2 failed because of no points in Traj2.");
            trajectory_buffer_1_requires_update = true;
            trajectory_buffer_2_requires_update = true;
        }

    }

    /// Check if replanning is necessary
    static float replanning_tracking_error_threshold = 0.5f;
    static float start_point_tracking_error_threshold = 0.4f;

    if(!trajectory_buffer_1_requires_update){
        Eigen::Vector3d tracking_p_error = current_p - last_send_p;
        if(tracking_p_error.norm() > replanning_tracking_error_threshold){
            trajectory_buffer_1_requires_update = true;
            trajectory_buffer_2_requires_update = true;
            ROS_WARN_THROTTLE(0.3, "Replan traj1 because of tracking error. current_p=(%f,%f), sp=(%f,%f)", current_p.x(), current_p.y(), traj_p_buffer_1[trajectory_1_send_counter-1].x(), traj_p_buffer_1[trajectory_1_send_counter-1].y());

        }else{
            //check collision, if collision, replan
            if(!collisionCheckingDynamic(left_points_piece_one, traj_p_buffer_1[trajectory_1_send_counter-1])){
                trajectory_buffer_1_requires_update = true;
                trajectory_buffer_2_requires_update = true;
                ROS_INFO_THROTTLE(0.3, "Replan traj1 because of collision.");

            }

        }
    }

    if(!trajectory_buffer_2_requires_update){
        float piece2_risk = 0.f;
        bool piece2_free_flag = collisionCheckingAndRiskPredictionDynamic(traj_p_buffer_2_matrix, traj_p_buffer_2_matrix.row(0), piece2_risk);

        if(!piece2_free_flag || piece2_risk > MAX_RISK_ONE_PRIMITIVE - 5.0){
            trajectory_buffer_2_requires_update = true;
            ROS_INFO_THROTTLE(0.3, "Replan traj2 because of risk.");
        }
    }

    /// Update piece 1
    if(trajectory_buffer_1_requires_update){
        Eigen::MatrixXd traj_p_buffer_chosen, traj_v_buffer_chosen, traj_a_buffer_chosen;

        Eigen::Vector3d start_p, start_v, start_a;
        if(trajectory_initialized){
            Eigen::Vector3d tracking_p_error = current_p - last_send_p;
            if(tracking_p_error.norm() > start_point_tracking_error_threshold){
                start_p = current_p * 0.7 + last_send_p * 0.3;
                start_v = current_v * 0.7 + last_send_v * 0.3;
                start_a = current_a * 0.7 + last_send_a * 0.3;
            }else{
                start_p = last_send_p;
                start_v = last_send_v;
                start_a = last_send_a;
            }
        }else{
            start_p = current_p;
            start_v = current_v;
            start_a = current_a;
        }

        ROS_INFO_THROTTLE(0.5, "Start v =(%f, %f, %f)", start_v(0), start_v(1), start_v(2));
        traj_p_buffer_1.clear();
        traj_v_buffer_1.clear();
        traj_a_buffer_1.clear();

        bool plan_successful_flag = trajectoryPlanOnePiece(start_p, start_v, start_a, current_yaw, traj_p_buffer_chosen, traj_v_buffer_chosen, traj_a_buffer_chosen, 1, absolute_safe_time);

        if(plan_successful_flag){
            int point_num = points_num_one_piece;
            if(point_num > traj_p_buffer_chosen.rows()){point_num = traj_p_buffer_chosen.rows();}


            for(int i=0; i<point_num; ++i){
                traj_p_buffer_1.emplace_back(traj_p_buffer_chosen.row(i));
                traj_v_buffer_1.emplace_back(traj_v_buffer_chosen.row(i));
                traj_a_buffer_1.emplace_back(traj_a_buffer_chosen.row(i));
                trajectory_1_send_counter = 0;
            }

//            whole_traj1_to_visualize = traj_p_buffer_chosen;

            trajectory_buffer_1_requires_update = false;
            safe_trajectory_avaliable = true;
            trajectory_initialized = true;
        }
        else{
            /// TODO: Should enter safety mode
            safe_trajectory_avaliable = false;
        }

    }

    /// Update piece 2
    if(trajectory_buffer_2_requires_update && safe_trajectory_avaliable){
        Eigen::MatrixXd traj_p_buffer_chosen, traj_v_buffer_chosen, traj_a_buffer_chosen;

        bool plan_successful_flag = trajectoryPlanOnePiece(traj_p_buffer_1[traj_p_buffer_1.size()-1], traj_v_buffer_1[traj_v_buffer_1.size()-1],
                                                           traj_a_buffer_1[traj_a_buffer_1.size()-1], yaw0, traj_p_buffer_chosen, traj_v_buffer_chosen, traj_a_buffer_chosen, 2, absolute_safe_time);
        traj_p_buffer_2.clear();
        traj_v_buffer_2.clear();
        traj_a_buffer_2.clear();

        if(plan_successful_flag){
            int point_num = points_num_one_piece;
            if(point_num > traj_p_buffer_chosen.rows()){point_num = traj_p_buffer_chosen.rows();}

            for(int i=0; i<point_num; ++i){
                traj_p_buffer_2.emplace_back(traj_p_buffer_chosen.row(i));
                traj_v_buffer_2.emplace_back(traj_v_buffer_chosen.row(i));
                traj_a_buffer_2.emplace_back(traj_a_buffer_chosen.row(i));
            }

            trajectory_buffer_2_requires_update = false;
            traj_p_buffer_2_matrix = traj_p_buffer_chosen;

//            whole_traj2_to_visualize = traj_p_buffer_chosen;
        }
    }


    marker_publish(traj_p_buffer_1, 1, 0.8f, 0.8f, 0.8f, 0.05, 8);
    marker_publish(traj_p_buffer_2, 2, 0.2f, 0.6f, 0.7f, 0.1, 10);

//    if(whole_traj1_to_visualize.rows() > 0) marker_publish(whole_traj1_to_visualize, 4, 0.6f, 0.5f, 0.1f, 0.06, 10);
//    if(whole_traj2_to_visualize.rows() > 0) marker_publish(whole_traj2_to_visualize, 5, 0.8f, 0.8f, 0.8f, 0.06, 10);

    double planning_end_time = ros::Time::now().toSec();
    ROS_INFO_THROTTLE(0.5, "Planning time is %lf s", planning_end_time-planning_start_time);
}


void sendCallback(const ros::TimerEvent& e)
{
    if(!trajectory_initialized){
        return;
    }

    static Eigen::Vector3d p_store_for_em;

    static bool init_flag = true;
    if(init_flag){
        p_store_for_em << p0(0), p0(1), p0(2);
        init_flag = false;
    }

    static float def_ori = pp.d_ref;

    trajectory_msgs::JointTrajectoryPoint pva_setpoint;

    if(safe_trajectory_avaliable){
        pp.d_ref = def_ori;

        float z_p_set, z_v_set, z_a_set;
        if(if_plan_vertical_path){
            z_p_set = traj_p_buffer_1[trajectory_1_send_counter](2);
            z_v_set = traj_v_buffer_1[trajectory_1_send_counter](2);
            z_a_set = traj_a_buffer_1[trajectory_1_send_counter](2);
        }
        else{
            z_p_set = p_goal(2);
            z_v_set = 0.f;
            z_a_set = 0.f;
        }

        pva_setpoint.positions.push_back(traj_p_buffer_1[trajectory_1_send_counter](0)); //x
        pva_setpoint.positions.push_back(traj_p_buffer_1[trajectory_1_send_counter](1)); //y
        pva_setpoint.positions.push_back(z_p_set); //z
        pva_setpoint.positions.push_back(0);  //yaw

        pva_setpoint.velocities.push_back(traj_v_buffer_1[trajectory_1_send_counter](0));
        pva_setpoint.velocities.push_back(traj_v_buffer_1[trajectory_1_send_counter](1));
        pva_setpoint.velocities.push_back(z_v_set);

        pva_setpoint.accelerations.push_back(traj_a_buffer_1[trajectory_1_send_counter](0));
        pva_setpoint.accelerations.push_back(traj_a_buffer_1[trajectory_1_send_counter](1));
        pva_setpoint.accelerations.push_back(z_a_set);

        p_store_for_em << p0(0), p0(1), p0(2);

        ++ trajectory_1_send_counter;
        if(trajectory_1_send_counter >= traj_a_buffer_1.size()){
            -- trajectory_1_send_counter;
            safe_trajectory_avaliable = false;
            ROS_ERROR_THROTTLE(0.1, "No points in the buffer. Use a larger the trajectory time or raise planning frequency.");
        }
        last_send_p = traj_p_buffer_1[trajectory_1_send_counter];
        last_send_v = traj_v_buffer_1[trajectory_1_send_counter];
        last_send_a = traj_a_buffer_1[trajectory_1_send_counter];

    }else{  //emergency stop
        pp.d_ref = 0.5;

        pva_setpoint.positions.push_back(p_store_for_em(0)); //x
        pva_setpoint.positions.push_back(p_store_for_em(1)); //y
        pva_setpoint.positions.push_back(p_store_for_em(2)); //z
        pva_setpoint.positions.push_back(0);  //yaw

        pva_setpoint.velocities.push_back(0);
        pva_setpoint.velocities.push_back(0);
        pva_setpoint.velocities.push_back(0);

        pva_setpoint.accelerations.push_back(0);
        pva_setpoint.accelerations.push_back(0);
        pva_setpoint.accelerations.push_back(0);

        ROS_WARN_THROTTLE(0.3, "Safety mode.");

        last_send_p = p_store_for_em;
        last_send_v << 0.0, 0.0, 0.0;
        last_send_a << 0.0, 0.0, 0.0;
    }

    pva_pub.publish(pva_setpoint);
}


static void split(const string& s, vector<string>& tokens, const string& delimiters = " ")
{
    string::size_type lastPos = s.find_first_not_of(delimiters, 0);
    string::size_type pos = s.find_first_of(delimiters, lastPos);
    while (string::npos != pos || string::npos != lastPos) {
        tokens.push_back(s.substr(lastPos, pos - lastPos));
        lastPos = s.find_first_not_of(delimiters, pos);
        pos = s.find_first_of(delimiters, lastPos);
    }
}


void actorStatesCallback(const gazebo_msgs::ModelStates &msg){
    actor_states.clear();
    for(int i=0; i<msg.name.size(); ++i)
    {
        vector<string> name_splited;
        split(msg.name[i], name_splited, "_");
        if(name_splited[0] == "actor"){
            ActorState a;
            a.name = msg.name[i];
            a.record_time = ros::Time::now().toSec();
            a.px = msg.pose[i].position.x;
            a.py = msg.pose[i].position.y;
            a.pz = msg.pose[i].position.z;

            float w = msg.pose[i].orientation.w;
            float x = msg.pose[i].orientation.x;
            float y = msg.pose[i].orientation.y;
            float z = msg.pose[i].orientation.z;

            float bb = 2 * (w * z + x * y);
            float cc = 1 - 2 * (y * y + z * z);
            a.yaw = std::atan2(bb, cc);
            actor_states.push_back(a);

            // Calculate velocity
            bool found_pair = false;
            for(auto &item : actor_avg_states){
                if(a.name == item.name){
                    found_pair = true;

                    double delt_t = a.record_time - item.record_time;

                    if(delt_t < 0.05){  //update not faster than 20 Hz
                        break;
                    }

                    double vx_this = (a.px - item.px) / delt_t;
                    double vy_this = (a.py - item.py) / delt_t;
                    double vz_this = (a.pz - item.pz) / delt_t;

                    if(fabs(item.vx) < 0.00001f){
                        item.vx = vx_this;
                        item.vy = vy_this;
                        item.vz = vz_this;
                    }else{
                        item.vx = (vx_this + item.vx) / 2.f;
                        item.vy = (vy_this + item.vy) / 2.f;
                        item.vz = (vz_this + item.vz) / 2.f;
                    }

                    item.record_time = a.record_time;
                    item.px = a.px;
                    item.py = a.py;
                    item.pz = a.pz;
                    item.yaw = a.yaw;
                }
            }
            if(!found_pair){
                actor_avg_states.push_back(a);
            }

        }
    }

    /// Publish
    vector<Eigen::Vector3d> actor_visualization_points;
    for(auto const &item : actor_avg_states){
//       cout << item.name<<", v=(" << item.vx << ", "<< item.vy << ", " << item.vz <<") ";
        Eigen::Vector3d p;
        p.x() = item.px;
        p.y() = item.py;
        p.z() = item.pz;
        actor_visualization_points.push_back(p);
    }
//    cout << endl;

    marker_publish(actor_visualization_points, 6, 1.f, 0.f, 0.f, 0.4, -1);

}


void positionCallback(const geometry_msgs::PoseStamped& msg)
{
    if(!state_locked)
    {
        state_updating = true;

        /** Change from ENU to NWU, NEEDS CAREFUL CHECKING!!!!, chg**/
        // p0(0) = msg.pose.position.y;
        // p0(1) = -msg.pose.position.x;
        // p0(2) = msg.pose.position.z;

        // quad.x() = msg.pose.orientation.x;
        // quad.y() = msg.pose.orientation.y;
        // quad.z() = msg.pose.orientation.z;
        // quad.w() = msg.pose.orientation.w;

        // //Eigen::Quaternionf q1(0, 0, 0, 1);
        // Eigen::Quaternionf axis; //= quad * q1 * quad.inverse();
        // axis.w() = cos(-PI_2/2.0);
        // //axis.x() = axis.x() * sin(-PI_2/2.0);
        // //axis.y() = axis.y() * sin(-PI_2/2.0);
        // //axis.z() = axis.z() * sin(-PI_2/2.0);
        // axis.x() = 0.0;
        // axis.y() = 0.0;
        // axis.z() = sin(-PI_2/2.0);
        // quad = quad * axis;

        // NWU
        p0(0) = msg.pose.position.x;
        p0(1) = msg.pose.position.y;
        p0(2) = msg.pose.position.z;

        quad.x() = msg.pose.orientation.x;
        quad.y() = msg.pose.orientation.y;
        quad.z() = msg.pose.orientation.z;
        quad.w() = msg.pose.orientation.w;

        // Queue for synchronization
        pose_timestamp_queue.push(msg.header.stamp.toSec());
        pos_queue.push(p0);
        att_queue.push(quad);

        if(pose_timestamp_queue.size()>200){
            pose_timestamp_queue.pop();
            pos_queue.pop();
            att_queue.pop();
        }


        /// Update yaw0 here, should be among [-PI, PI]
        // yaw0 = atan2(2*(quad.w()*quad.z()+quad.x()*quad.y()), 1-2*(quad.z()*quad.z()+quad.y()*quad.y()));   /// TODO: CHECK if this is right!!!
        yaw0 = atan2(2*(quad.w()*quad.z()+quad.x()*quad.y()), 1-2*(quad.z()*quad.z()+quad.y()*quad.y()));


        state_updating = false;
    }

//    if(!offboard_ready){
//        init_p = p0;  //record a base point
//        p_goal(0) = p_goal_raw(0) + init_p(0);
//        p_goal(1) = p_goal_raw(1) + init_p(1);
//        p_goal(2) = init_p(2);
//        ROS_INFO_THROTTLE(1.0, "Waiting for offboard mode. p_goal = %f, %f, %f", p_goal(0), p_goal(1), p_goal(2));
//    }else{
//        ROS_INFO_THROTTLE(1.0, "IN offboard mode. pgoal = %f, %f, %f", p_goal(0), p_goal(1), p_goal(2));
//    }
//
//    ROS_INFO_THROTTLE(1.0, "Current pose. p0 = %f, %f, %f", p0(0), p0(1), p0(2));
}

void velocityCallback(const geometry_msgs::TwistStamped& msg)
{
    if(!state_locked)
    {
        state_updating = true;

        // /** Change from ENU to NWU, NEEDS CAREFUL CHECKING!!!!, chg**/
        // v0(0) = msg.twist.linear.y;
        // v0(1) = -msg.twist.linear.x;
        // v0(2) = msg.twist.linear.z;
        // yaw0_rate = msg.twist.angular.z;

        /**  NWU  **/
        v0(0) = msg.twist.linear.x;
        v0(1) = msg.twist.linear.y;
        v0(2) = msg.twist.linear.z;
        yaw0_rate = msg.twist.angular.z;

        if(fabs(v0(0)) < 0.05) v0(0) = 0.0;  //add a dead zone for v0 used in motion primatives
        if(fabs(v0(1)) < 0.05) v0(1) = 0.0;  //add a dead zone for v0 used in motion primatives
        if(fabs(v0(2)) < 0.05) v0(2) = 0.0;  //add a dead zone for v0 used in motion primatives

        float max_current_v = 0.8;
        for(int i=0; i<3; i++){
            if(v0(i) < -max_current_v){
                v0(i) = -max_current_v;
            }else if(v0(i) > max_current_v){
                v0(i) = max_current_v;
            }
        }

        /** Calculate virtual accelerates from velocity. Original accelerates given by px4 is too noisy **/
        static bool init_v_flag = true;
        static float last_time, last_vx, last_vy, last_vz;

        if(init_v_flag){
            init_v_flag = false;
        }
        else{
            double delt_t = ros::Time::now().toSec() - last_time;
            a0(0) = (v0(0) - last_vx) / delt_t;
            a0(1) = (v0(1) - last_vy) / delt_t;
            a0(2) = (v0(2) - last_vz) / delt_t;

            if(fabs(a0(0)) < 0.2) a0(0) = 0.0;  //dead zone for acc x
            if(fabs(a0(1)) < 0.2) a0(1) = 0.0; //dead zone for acc y
            if(fabs(a0(2)) < 0.2) a0(2) = 0.0; //dead zone for acc z

            float max_current_a = 1.0;
            for(int i=0; i<3; i++){
                if(a0(i) < -max_current_a){
                    a0(i) = -max_current_a;
                }else if(a0(i) > max_current_a){
                    a0(i) = max_current_a;
                }
            }

            //ROS_INFO("acc=(%f, %f, %f)", a0(0), a0(1), a0(2));
        }

        last_time = ros::Time::now().toSec();
        last_vx = v0(0);
        last_vy = v0(1);
        last_vz = v0(2);

        state_updating = false;
    }
}


void marker_publish(const Eigen::MatrixXd &Points, int id, float r, float g, float b, float width, int publish_num)
{
    if(Points.rows() < 1) return;

    visualization_msgs::Marker points;
    points.header.frame_id = "map";
    points.header.stamp = ros::Time::now();
    points.action = visualization_msgs::Marker::ADD;
    points.ns = "lines_and_points";
    points.id = id;
    points.type = visualization_msgs::Marker::POINTS;

    // Line width
    points.scale.x = width;
    points.scale.y = width;

    points.color.r = r;
    points.color.g = g;
    points.color.b = b;
    points.color.a = 1.0;
    points.lifetime = ros::Duration(0);

    int step = 1;
    if(publish_num > 0){
        step = (int)Points.rows() / publish_num;
    }

    for(int i=0; i<Points.rows(); i+=step)
    {
        geometry_msgs::Point p;
        p.x = Points(i, 0);
        p.y = Points(i, 1);
        p.z = Points(i, 2);

        points.points.push_back(p);
    }

    geometry_msgs::Point p;
    p.x = Points(Points.rows()-1, 0);
    p.y = Points(Points.rows()-1, 1);
    p.z = Points(Points.rows()-1, 2);
    points.points.push_back(p);

    current_marker_pub.publish(points);
}


void marker_publish(const vector<Eigen::Vector3d> &Points, int id, float r, float g, float b, float width, int publish_num)
{
    if(Points.empty()) return;

    visualization_msgs::Marker points;
    points.header.frame_id = "map";
    points.header.stamp = ros::Time::now();
    points.action = visualization_msgs::Marker::ADD;
    points.ns = "lines_and_points";
    points.id = id;
    points.type = visualization_msgs::Marker::POINTS;

    // Line width
    points.scale.x = width;
    points.scale.y = width;

    points.color.r = r;
    points.color.g = g;
    points.color.b = b;
    points.color.a = 1.0;
    points.lifetime = ros::Duration(0);

    int step = 1;
    if(publish_num > 0){
        step = Points.size() / publish_num;
    }
    step = max(1, step);

    for(int i=0; i<Points.size(); i+=step)
    {
        geometry_msgs::Point p;
        p.x = Points[i](0);
        p.y = Points[i](1);
        p.z = Points[i](2);
        points.points.push_back(p);
    }

    geometry_msgs::Point p;
    p.x = Points[Points.size()-1](0);
    p.y = Points[Points.size()-1](1);
    p.z = Points[Points.size()-1](2);
    points.points.push_back(p);

    current_marker_pub.publish(points);
}



int main(int argc, char** argv) {

    ros::init(argc, argv, "planning_test");
    ros::NodeHandle nh;

    setParameters();    /// chg, use parameters defined here

    trajectory_time_table->csv2pva_table("/home/clarence/ros_ws/map_ws/src/tables/p3-2_v3_a4_res0-1.csv");
    ROS_WARN("trajectory_time_table loaded!");

    p0 << 0.0, 0.0, 0.0;
    v0 << 0.0, 0.0, 0.0;
    a0 << 0.0, 0.0, 0.0;
//    p_goal << -1.0, -5.0, 1.2;
    yaw0 = 0.0;

    // Fov sample parameters
    Fov_half << 35, 20;
    // Horizontal angles larger than 90 degree are deleted
    //Angle_h << -90, -70, -50, -30, -20, -10, 0, 10, 20, 30, 50, 70, 90;
    if(if_flyback_enable){
//        Angle_h << -130, -90, -75, -60, -45, -30, -25, -20, -15, -10, -5, 0, 5, 10, 15, 20, 25, 30, 45, 60, 75, 90, 130;
        Angle_h << -120, -112, -104, -96, -88, -80, -72, -64, -56, -48, -40, -32, -24, -16, -8, 0, 8, 16, 24, 32, 40, 48, 56, 64, 72, 80, 88, 96, 104, 112, 120;
    }else{
//        Angle_h << -90, -80, -70, -60, -50, -40, -30, -20, -10, 0, 5, 10, 20, 30, 40, 50, 60, 70, 80, 90;
        Angle_h << -60, -56, -48, -40, -32, -24, -16, -8, 0, 8, 16, 24, 32, 40, 48, 56, 60;

    }

    if(if_plan_vertical_path){
        // Angle_v << -60, -56, -48, -40, -32, -24, -16, -8, 0, 8, 16, 24, 32, 40, 48, 56, 60;
        Angle_v << -24, -16, -8, 0, 8, 16, 24;
    } else{
        Angle_v << 0, 0, 0, 0, 0, 0, 0;
    }

    if(Angle_h.size() != ANGLE_H_NUM || Angle_v.size() != ANGLE_V_NUM){
        ROS_INFO(" ANGLE_H_NUM or ANGLE_V_NUM mismatches the initialized vector. Please check!");
        return 0;
    }

    Angle_h = Angle_h * M_PI / 180.0; // degree to rad
    Angle_v = Angle_v * M_PI / 180.0;
    Fov_half = Fov_half * M_PI / 180.0;

    pva_pub = nh.advertise<trajectory_msgs::JointTrajectoryPoint>("/pva_setpoint", 1, true);
    current_marker_pub = nh.advertise<visualization_msgs::Marker>("/visualization_marker", 1);

    ros::Subscriber pedestrian_sub = nh.subscribe("/gazebo/model_states", 1, actorStatesCallback);
    ros::Subscriber position_isolate_sub =  nh.subscribe("/mavros/local_position/pose", 1, positionCallback);
    ros::Subscriber velocity_isolate_sub = nh.subscribe("/mavros/local_position/velocity_local", 1, velocityCallback);

//    ros::Subscriber cloud_sub = nh.subscribe("/camera_front/depth/points", 1, cloudCallback);

    ros::Timer timer1 = nh.createTimer(ros::Duration(0.1), trajectoryCallback);

    ros::Timer timer2 = nh.createTimer(ros::Duration(CAL_DURATION), sendCallback);


//    ros::spin();
    ros::AsyncSpinner spinner(3); // Use 3 threads
    spinner.start();
    ros::waitForShutdown();

    return 0;
}


void setParameters(){
    p_goal(0) = 20.0;
    p_goal(1)= 0.0;
    p_goal(2)=1.2;
    p_goal_raw = p_goal;

    MAX_V_XY = 1.5;
    MAX_V_Z_UP = 0.8;
    MAX_V_Z_DOWN = 0.4;
    MAX_A = 2;
    trajectory_1_send_counter = 1;  // To eliminate the influence of delay
    pp.d_ref = 3.0;
    pp.k1_xy = 4.0;
    pp.k1_z = 4.0;
    pp.k2_xy = 1.0;
    pp.k2_z = 1.0;
    pp.v_max_at_goal_ori = 0.8;
    pp.collision_threshold_static = 0.5;
    pp.collision_threshold_dynamic = 1.0;

    if_plan_vertical_path = false;
    if_in_simulation = true;
    if_flyback_enable = false;
    HEIGHT_LIMIT = 2.8;
    XY_LIMIT = 2.2;
    PLAN_INIT_STATE_CHANGE_THRESHOLD = 0.1;
    DIRECTION_CHANGE_LEFT_SIZE = 1.5;
    DIRECTION_AUTO_CHANGE = false;
    MAP_DELAY_SECONDS = 0.05;
}